/*
 * sccilp.c, SmartCam coprocessor library for XeTaL (1)
 *
 */

#define _USE_MATH_DEFINES

#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <math.h>
#include <Rhapsody.h>
#include <RapError.h>
 
#include "XetalGlobals.h"
#include "Xetal.h"

#include "xtltest.h"

void xtlWriteCommand(int opid)
{
  /* Only change registers which matter */
  d_i2c[11] = 0;      // GAIN MSB
  d_i2c[12] = opid;   // GAIN LSB

  TrmI2cWriteBlock( 0x18, 13, d_i2c );
}

void Main()
{
  IMAGE imageDisplay, imageOverlay;

  RAP_CALL( RapOpen(LCM_FILENAME) );
  RAP_CALL( RapInitialise() );

  RAP_CALL( XetalInactivateI2C() );
  
  // configure and enable the display and overlay
  RAP_CALL
  ( 
	  RapDisplayMode( RAP_DISPLAY_640x480x60HZ,
                      RAP_OVERLAY_100_PERCENT,
					  SENSOR_HEIGHT,
					  SENSOR_WIDTH,
                      &imageDisplay,
                      &imageOverlay)
  );

  RAP_CALL( RapDisplayEnable( RAP_ON, RAP_OFF) );

  // Define the Pixelcam sensor (=default for Inca).
  RAP_CALL( RapCameraDefine (RAP_PCS_2112_8_BITS, RAP_CONTINUOUS_MODE, RAP_NONE ) );
  
  // Set the integration time.
  RAP_CALL( RapCameraSetPar( RAP_PAR_INTEGRATION_SIZE, NR_INTEGRATION_LINES) );
  
  // Set Gain
  RAP_CALL( RapCameraSetPar( RAP_PAR_GAIN_CHANNEL_A, 0) );
  
  // Set the sensor in normal capture mode (=default);                    
  RAP_CALL( RapSetTimeout( RAP_TIMEOUT_CAPTURE, 1000) );

  RAP_CALL( RapCameraSetPar( RAP_PAR_OUTPUT_MODE, RAP_MODE_NORMAL /* RAP_MODE_TEST */ ) );

  RAP_CALL( RapCameraSetPar( RAP_PAR_HORIZONTAL_DIRECTION, RAP_DIRECTION_MIRRORED ) );

  // Set pixelclock sensor
  RAP_CALL( RapCameraSetPar (RAP_PAR_FREQUENCY, 16) );

  // Xetal stuff                                
  RAP_CALL( XetalActivateI2C() );
  RAP_CALL( XetalLoadProgram("/flash/xtltest.cmd") );
  RAP_CALL( XetalWriteParam() );
  RAP_CALL( XetalOn() );

  // Continuous capture
  RAP_CALL ( RapCaptureExtended(  	RAP_CHANNEL_A,          /* Must be channel A */
                                   	RAP_CAPTURE_FRAME | RAP_CAPTURE_CONTINUOUS,      /* Continuous Capture a frame */
                                  	0, 
                                    RAP_CAPTURE_NOWAIT,
									SENSOR_STARTROW,
									SENSOR_STARTCOL,
									SENSOR_HEIGHT,
									SENSOR_WIDTH,
	                                SUBSAMPLE_FACTOR,
	                                SUBSAMPLE_FACTOR,
                                    NULL,
                                    imageDisplay) );

  while (1)
  {
    xtlWriteCommand(0);
    xtlWriteCommand(255);
  }
}
